/************************************************************************
 * @version   --> v1.0
 * @date      --> 2021-04-18
 * @brief     --> 热控模块
 *            1.把当前磁矩板、电源板、飞轮板、热控板上的温度传感器收集的数据
 *              通过串口发送给上位机给上位机，调试时进行串口输出
 *            2.通过串口接收上位机发送的指令，控制磁矩板、电源板、飞轮板、
 *              热控板加热电阻的开关，进行温度控制
 *@update     --> 2021-05-12 
 *@brief      --> 使用rosserial
 ***********************************************************************/

/*Includes ------------------------------------------------------------*/
#include <ros.h>
#include <learning_test/TCS.h>
#include <std_msgs/UINT16.h>
#include <std_msgs/Float32.h>
#include <string.h>                 // 使用内存拷贝
#include <SCoop.h>                  // 使用 SCoop 线程库
#include <MsTimer2.h>            // 使用定时器库
#include <OneWire.h>               // 单线程
#include <DallasTemperature.h>     // DS18B20

/* Private variables --------------------------------------------------*/
// 引脚定义
int SW0 = A7;     // 电源板加热开关引脚
int SW1 = A8;     // 飞轮板加热开关引脚
int SW2 = A9;     // 负载板加热开关引脚
int SW3 = A10;    // 磁矩板加热开关引脚
int SW4 = A11;    // 热控板加热开关引脚
const uint8_t DS18B20_ter = 46;   // 热控板温度，DS18B20数据口引脚
const uint8_t DS18B20_fw = 47;    // 飞轮板温度，DS18B20数据口引脚
const uint8_t DS18B20_pow = 48;   // 电源板温度，DS18B20数据口引脚
const uint8_t DS18B20_mag = 49;   // 磁矩板温度，DS18B20数据口引脚

// 开关状态记录
volatile bool P0_on = false;
volatile bool P1_on = false;
volatile bool P2_on = false;
volatile bool P3_on = false;
volatile bool P4_on = false;

// 温度设定值
volatile float temperature_mag_setting = 38.0;    // 磁矩板设定温度
volatile float temperature_pow_setting = 38.0;    // 电源板设定温度
volatile float temperature_fw_setting = 38.0;     // 飞轮板设定温度
volatile float temperature_ter_setting = 38.0;    // 热控板设定温度

// 温度测量值
volatile float temperature_mag_measurement = 0;   // 磁矩板测量温度
volatile float temperature_pow_measurement = 0;   // 电源板测量温度
volatile float temperature_fw_measurement = 0;    // 飞轮板测量温度
volatile float temperature_ter_measurement = 0;   // 热控板测量温度
// ISR（中断服务程序）中所涉及的变量需要被声明为volatile易变变量
// 状态控制标志量，温度控制
volatile bool thermal_control_on = false;

ros::NodeHandle nh;
learning_test::TCS measurement_msg;
std_msgs::UInt16 thermal_msg;
std_msgs::UInt16 pow_msg;
std_msgs::UInt16 ter_msg;
std_msgs::UInt16 fw_msg;
std_msgs::UInt16 mag_msg;
std_msgs::UInt16 pay_msg;
std_msgs::Float32 setting_pow;
std_msgs::Float32 setting_ter;
std_msgs::Float32 setting_fw;
std_msgs::Float32 setting_mag;

void subscriberCallback0(const std_msgs::UInt16& pow_sw){
    // 电源板热控开关
  if(pow_sw.data == 1){
    digitalWrite(SW0, HIGH);
    P0_on = true;
    }
  else{
    digitalWrite(SW0, LOW);
    P0_on = false;
    }
}

void subscriberCallback1(const std_msgs::UInt16& ter_sw){
    // 热控板热控开关
  if(ter_sw.data == 1){
    digitalWrite(SW4, HIGH);
    P4_on = true;
    }
  else{
    digitalWrite(SW4, LOW);
    P4_on = false;
    }
}

void subscriberCallback2(const std_msgs::UInt16& fw_sw){
    // 飞轮板热控开关
  if(fw_sw.data == 1){
    digitalWrite(SW1, HIGH);
    P1_on = true;
    }
  else{
    digitalWrite(SW1, LOW);
    P1_on = false;
    }
}

void subscriberCallback3(const std_msgs::UInt16& mag_sw){
    // 磁矩板热控开关
  if(mag_sw.data == 1){
    digitalWrite(SW3, HIGH);
    P3_on = true;
    }
  else{
    digitalWrite(SW3, LOW);
    P3_on = false;
    }
}

void subscriberCallback4(const std_msgs::UInt16& pay_sw){
    // 负载板热控开关
  if(pay_sw.data == 1){
    digitalWrite(SW2, HIGH);
    P2_on = true;
    }
  else{
    digitalWrite(SW2, LOW);
    P2_on = false;
    }
}

void subscriberCallback5(const std_msgs::Float32& setting_pow){
    // 电源板温度设定
    temperature_pow_setting = setting_pow.data;
}

void subscriberCallback6(const std_msgs::Float32& setting_ter){
    // 热控板温度设定
    temperature_ter_setting = setting_ter.data;
}

void subscriberCallback7(const std_msgs::Float32& setting_fw){
    // 飞轮板温度设定
    temperature_fw_setting = setting_fw.data;
}

void subscriberCallback8(const std_msgs::Float32& setting_mag){
    // 磁矩板温度设定
    temperature_mag_setting = setting_mag.data;
}

void subscriberCallback9(const std_msgs::UInt16& thermal_sw){
  // 是否打开温度控制开关
  if(thermal_sw.data == 1){
    thermal_control_on = true;
  }else{thermal_control_on = false;}
}

ros::Publisher TCS_publisher("TCS_state", &measurement_msg);
ros::Subscriber<std_msgs::UInt16> pow_subscriber("pow_sw", &subscriberCallback0);
ros::Subscriber<std_msgs::UInt16> ter_subscriber("ter_sw", &subscriberCallback1);
ros::Subscriber<std_msgs::UInt16> fw_subscriber("fw_sw", &subscriberCallback2);
ros::Subscriber<std_msgs::UInt16> mag_subscriber("mag_sw", &subscriberCallback3);
ros::Subscriber<std_msgs::UInt16> pay_subscriber("pay_sw", &subscriberCallback4);
ros::Subscriber<std_msgs::Float32> pow_setting_subscriber("setting_pow", &subscriberCallback5);
ros::Subscriber<std_msgs::Float32> ter_setting_subscriber("setting_ter", &subscriberCallback6);
ros::Subscriber<std_msgs::Float32> fw_setting_subscriber("setting_fw", &subscriberCallback7);
ros::Subscriber<std_msgs::Float32> mag_setting_subscriber("setting_mag", &subscriberCallback8);
ros::Subscriber<std_msgs::UInt16> thermal_subscriber("thermal_sw", &subscriberCallback9);

// 实例化温度传感器
OneWire oneWire1(DS18B20_mag);
DallasTemperature sensor1(&oneWire1);    // 声明连接在单总线上的单总线设备
OneWire oneWire2(DS18B20_pow);           // 声明一个传感器对象
DallasTemperature sensor2(&oneWire2);
OneWire oneWire3(DS18B20_fw);
DallasTemperature sensor3(&oneWire3);
OneWire oneWire4(DS18B20_ter);
DallasTemperature sensor4(&oneWire4);

// 计时器函数
void pertime()
{
  // 获取传感器摄氏温度数据
  temperature_mag_measurement = sensor1.getTempCByIndex(0);
  temperature_pow_measurement = sensor2.getTempCByIndex(0);
  temperature_fw_measurement = sensor3.getTempCByIndex(0);
  temperature_ter_measurement = sensor4.getTempCByIndex(0);
  // 向总线上的设备发送新一轮的温度转换请求
  sensor1.requestTemperatures();
  sensor2.requestTemperatures();
  sensor3.requestTemperatures();
  sensor4.requestTemperatures();
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(57600);   // 使用串口通信
  delay(20);
  // 热控开关
  pinMode(SW0, OUTPUT);
  pinMode(SW1, OUTPUT);
  pinMode(SW2, OUTPUT);
  pinMode(SW3, OUTPUT);
  pinMode(SW4, OUTPUT);
  // 温度传感器初始化
  sensor1.begin();
  sensor1.setWaitForConversion(false);    // 设置为非阻塞模式
  sensor1.requestTemperatures();
  sensor2.begin();
  sensor2.setWaitForConversion(false);    // 设置为非阻塞模式
  sensor2.requestTemperatures();
  sensor3.begin();
  sensor3.setWaitForConversion(false);    // 设置为非阻塞模式
  sensor3.requestTemperatures();
  sensor4.begin();
  sensor4.setWaitForConversion(false);    // 设置为非阻塞模式
  sensor4.requestTemperatures();
  
  // 定时器中断
  MsTimer2::set(500, pertime);    // 定时器定时500ms，更新一次传感器数据
  MsTimer2::start();
  nh.initNode();
  nh.advertise(TCS_publisher);
  nh.subscribe(pow_subscriber);
  nh.subscribe(ter_subscriber);
  nh.subscribe(fw_subscriber);
  nh.subscribe(mag_subscriber);
  nh.subscribe(pay_subscriber);
  nh.subscribe(pow_setting_subscriber);
  nh.subscribe(ter_setting_subscriber);
  nh.subscribe(fw_setting_subscriber);
  nh.subscribe(mag_setting_subscriber);
  nh.subscribe(thermal_subscriber);
}

void loop() {
  // 维持各系统在设定温度上下保持恒定
  if(thermal_control_on){
    if(temperature_pow_measurement <= temperature_pow_setting-2){
      digitalWrite(SW0, HIGH);
     }else if(temperature_pow_measurement > temperature_pow_setting+2){
      digitalWrite(SW0, LOW);
    }else{digitalWrite(SW0, LOW);}
    
    if(temperature_fw_measurement < temperature_fw_setting-2){
      digitalWrite(SW1, HIGH);
    }else if(temperature_fw_measurement > temperature_fw_setting+2){
      digitalWrite(SW1, LOW);
    }else{digitalWrite(SW1, LOW);}
    
    if(temperature_mag_measurement < temperature_mag_setting-2){
      digitalWrite(SW3, HIGH);
    }else if(temperature_mag_measurement > temperature_mag_setting+2){
      digitalWrite(SW3, LOW);
    }else{digitalWrite(SW3, LOW);}
    
    if(temperature_ter_measurement < temperature_ter_setting-2){
      digitalWrite(SW4, HIGH);
    }else if(temperature_ter_measurement > temperature_ter_setting+2){
      digitalWrite(SW4, LOW);
    }else{digitalWrite(SW4, LOW);}
  }
  Serial.println("此时各模块热控工作状态：");
  Serial.print("电源板热控开关：");
  Serial.println(P0_on);
  Serial.print("飞轮板板热控开关：");
  Serial.println(P1_on);
  Serial.print("负载板热控开关：");
  Serial.println(P2_on);
  Serial.print("磁矩板热控开关：");
  Serial.println(P3_on);
  Serial.print("热控板热控开关：");
  Serial.println(P4_on);
  Serial.print("\n");
  Serial.println("各模块设定温度：");
  Serial.print("磁矩板温度设定值：");
  Serial.println(temperature_mag_setting);
  Serial.print("电源板温度设定值：");
  Serial.println(temperature_pow_setting);
  Serial.print("飞轮板温度设定值：");
  Serial.println(temperature_fw_setting);
  Serial.print("热控板温度设定值：");
  Serial.println(temperature_ter_setting);
  Serial.print("\n");
  Serial.println("各模块测量温度：");
  Serial.print("磁矩板测量温度：");
  Serial.print(temperature_mag_measurement);
  Serial.println(" ℃");
  Serial.print("电源板测量温度：");
  Serial.print(temperature_pow_measurement);
  Serial.println(" ℃");
  Serial.print("飞轮板板测量温度：");
  Serial.print(temperature_fw_measurement);
  Serial.println(" ℃");
  Serial.print("温控板测量温度：");
  Serial.print(temperature_ter_measurement);
  Serial.println(" ℃");
  measurement_msg.pow_temperature = temperature_pow_measurement;
  measurement_msg.ter_temperature = temperature_ter_measurement;
  measurement_msg.fw_temperature = temperature_fw_measurement;
  measurement_msg.mag_temperature = temperature_mag_measurement;
  measurement_msg.pow_sw = P0_on;
  measurement_msg.ter_sw = P4_on;
  measurement_msg.fw_sw = P1_on;
  measurement_msg.mag_sw = P3_on;
  measurement_msg.pay_sw = P2_on;
  TCS_publisher.publish(&measurement_msg);
  nh.spinOnce();
  delay(20);
}
